#include "bodypart_rigid.h"

// BodypartRigid::BodypartRigid(boost::shared_ptr<btDynamicsWorld> ownerWorld, void* owner, btCollisionShape* nshape, float weight, btTransform& transform, const Color& clr, bodypartShape sh)
BodypartRigid::BodypartRigid(bodypartShape sh, const BeColor& clr, boost::shared_ptr<btDynamicsWorld> ownerWorld)
 : Bodypart( sh, RIGID, clr, ownerWorld )
{
}

void BodypartRigid::generalSetup( btVector3& localInertia, float weight, const btTransform& transform, void* owner, float lin_damping, float ang_damping )
{
	if (weight != 0.f) // weight of non zero = dynamic
		shape->calculateLocalInertia( weight, localInertia );

	m_motionState = boost::shared_ptr<btDefaultMotionState>(new btDefaultMotionState( transform ));
	
	btRigidBody::btRigidBodyConstructionInfo rbInfo( weight, m_motionState.get(), shape, localInertia );
	m_body = boost::shared_ptr<btRigidBody>(new btRigidBody(rbInfo));

	m_body->setUserPointer(owner);
	m_body->setDamping(lin_damping, ang_damping);
	m_body->setDeactivationTime(0.0001);
	m_body->setSleepingThresholds(0.36, 0.05);
	m_ownerWorld->addRigidBody(m_body.get());
}

void BodypartRigid::updatePosition()
{
	m_motionState->m_graphicsWorldTrans.getOpenGLMatrix(drawposition);
}

// void BodypartRigid::copyPosition()
// {
// 	for ( unsigned int i=0; i < 16; i++ )
// 		drawposition[i] = position[i];
// }

BodypartRigid::~BodypartRigid()
{
	// Remove all bodies and shapes
	m_ownerWorld->removeRigidBody(m_body.get());
}


